#include "systemexcep.h"

static StackFrame_t frame = {0};

void systemHardFaultErr(uint32_t *reg)
{
    printf("System Error\n");

    // 输出寄存器R0,R1,PC ,SP 值

    printf("MSP = %#x\r\n", __get_MSP());
    printf("PSP = %#x\r\n", __get_PSP());
    // printf("APSR = %#x\r\n", __get_APSR());
    // printf("BASEFPI = %#x\r\n", __get_BASEPRI());
    // printf("CONTROL = %#x\r\n", __get_CONTROL());
    // // printf("LR = %#x\r\n", __get_LR());
    // printf("R0 = %#x\r\n", getR0value());
    // printf("R1 = %#x\r\n", getR1value());
    // printf("PC = %#x\r\n", getPCvalue());
    // printf("SP = %#x\r\n", getSPvalue());

    // // 输出当前栈内容
    // uint32_t *p = (uint32_t *)__get_MSP();
    // for (int i = 0; i < 16; i++)
    // {
    //     printf("p[%#x]=%#x\r\n", p - i, *(p - i));
    // }

    // printf("----------------------------------------\r\n");
    // for (int i = 0; i < 16; i++)
    // {
    //     printf("p[%#x]=%#x\r\n", p + i, *(p + i));
    // }

    // printf("----------------------------------------\r\n");

    memcpy(&frame, (uint32_t *)__get_MSP(), sizeof(StackFrame_t));
    printf("R0 = %#x\r\n", frame.R0);
    printf("R1 = %#x\r\n", frame.R1);
    printf("R2 = %#x\r\n", frame.R2);
    printf("R3 = %#x\r\n", frame.R3);
    printf("R12 = %#x\r\n", frame.R12);
    printf("LR = %#x\r\n", frame.LR);
    printf("PC = %#x\r\n", frame.PC);
    printf("xPSR = %#x\r\n", frame.xPSR);

    memset(&frame, 0, sizeof(StackFrame_t));
    printf("---------------------PSP-------------------\r\n");
    memcpy(&frame, (uint32_t *)__get_PSP(), sizeof(StackFrame_t));
    printf("R0 = %#x\r\n", frame.R0);
    printf("R1 = %#x\r\n", frame.R1);
    printf("R2 = %#x\r\n", frame.R2);
    printf("R3 = %#x\r\n", frame.R3);
    printf("R12 = %#x\r\n", frame.R12);
    printf("LR = %#x\r\n", frame.LR);
    printf("PC = %#x\r\n", frame.PC);
    printf("xPSR = %#x\r\n", frame.xPSR);

    printf("---------------------PSP-------------------\r\n");

    for (size_t i = 0; i < 15; i++)
    {
        printf("reg[%d] = %#x\r\n", i, reg[i]);
    }

    printf("---------------------SYSTEM REBOOT -------------------\r\n");

    // 重启系统
    // NVIC_SystemReset();

    // HAL_NVIC_SystemReset();

    // 跳转到 0x80000000
    // #if 1
    //     __ASM volatile("LDR R0, =0x80000000\n"
    //                    "BX  R0\n");
    // #else
    //     while (1)
    //         ;
    // #endif
    //     // __NVIC_SetVector(0x8000000);
}

void hard_fault_handler_c(unsigned int *hardfault_args)
{
    unsigned int stacked_r0;
    unsigned int stacked_r1;
    unsigned int stacked_r2;
    unsigned int stacked_r3;
    unsigned int stacked_r12;
    unsigned int stacked_lr;
    unsigned int stacked_pc;
    unsigned int stacked_psr;
    stacked_r0 = ((unsigned long)hardfault_args[0]);
    stacked_r1 = ((unsigned long)hardfault_args[1]);
    stacked_r2 = ((unsigned long)hardfault_args[2]);
    stacked_r3 = ((unsigned long)hardfault_args[3]);
    stacked_r12 = ((unsigned long)hardfault_args[4]);
    stacked_lr = ((unsigned long)hardfault_args[5]);
    stacked_pc = ((unsigned long)hardfault_args[6]);
    stacked_psr = ((unsigned long)hardfault_args[7]);
    printf("\n\n[Hard fault handler - all numbers in hex]\n");
    printf("R0 = %#x\n", stacked_r0);
    printf("R1 = %#x\n", stacked_r1);
    printf("R2 = %#x\n", stacked_r2);
    printf("R3 = %#x\n", stacked_r3);
    printf("R12 = %#x\n", stacked_r12);
    printf("LR [R14] = %#x subroutine call return address\n", stacked_lr);
    printf("PC [R15] = %#x program counter\n", stacked_pc);
    printf("PSR = %#x\n", stacked_psr);
    printf("BFAR = %#x\n", (*((volatile unsigned long *)(0xE000ED38))));
    printf("CFSR = %#x\n", (*((volatile unsigned long *)(0xE000ED28))));
    printf("HFSR = %#x\n", (*((volatile unsigned long *)(0xE000ED2C))));
    printf("DFSR = %#x\n", (*((volatile unsigned long *)(0xE000ED30))));
    printf("AFSR = %#x\n", (*((volatile unsigned long *)(0xE000ED3C))));
    printf("SCB_SHCSR = %#x\n", SCB->SHCSR);
    while (1)
        ;
}
